#ifndef _MATRIX_H_
#define _MATRIX_H_

#include "types.h"
#include "vector.h"
#include <math.h>
#include <memory.h>

class matrix4
{
public:
	matrix4()
	{
		memset(mat,0,16*4);
	}

	 __forceinline matrix4 operator*(matrix4& _mtx)
	{
		matrix4 ret;

		ret.mat[0] = mat[0] *  _mtx.mat[0] +  mat[4] * _mtx.mat[1] +  mat[8] *  _mtx.mat[2] +  mat[12] * _mtx.mat[3];
		ret.mat[1] = mat[1] *  _mtx.mat[0] +  mat[5] * _mtx.mat[1] +  mat[9] *  _mtx.mat[2] +  mat[13] * _mtx.mat[3];
		ret.mat[2] = mat[2] *  _mtx.mat[0] +  mat[6] * _mtx.mat[1] +  mat[10] * _mtx.mat[2] +  mat[14] * _mtx.mat[3];
		ret.mat[3] = mat[3] *  _mtx.mat[0] +  mat[7] * _mtx.mat[1] +  mat[11] * _mtx.mat[2] +  mat[15] * _mtx.mat[3];
		ret.mat[4] = mat[0] *  _mtx.mat[4] +  mat[4] * _mtx.mat[5] +  mat[8] *  _mtx.mat[6] +  mat[12] * _mtx.mat[7];
		ret.mat[5] = mat[1] *  _mtx.mat[4] +  mat[5] * _mtx.mat[5] +  mat[9] *  _mtx.mat[6] +  mat[13] * _mtx.mat[7];
		ret.mat[6] = mat[2] *  _mtx.mat[4] +  mat[6] * _mtx.mat[5] +  mat[10] * _mtx.mat[6] +  mat[14] * _mtx.mat[7];
		ret.mat[7] = mat[3] *  _mtx.mat[4] +  mat[7] * _mtx.mat[5] +  mat[11] * _mtx.mat[6] +  mat[15] * _mtx.mat[7];
		ret.mat[8] = mat[0] *  _mtx.mat[8] +  mat[4] * _mtx.mat[9] +  mat[8] *  _mtx.mat[10] + mat[12] * _mtx.mat[11];
		ret.mat[9] = mat[1] *  _mtx.mat[8] +  mat[5] * _mtx.mat[9] +  mat[9] *  _mtx.mat[10] + mat[13] * _mtx.mat[11];
		ret.mat[10] = mat[2] * _mtx.mat[8] +  mat[6] * _mtx.mat[9] +  mat[10] * _mtx.mat[10] + mat[14] * _mtx.mat[11];
		ret.mat[11] = mat[3] * _mtx.mat[8] +  mat[7] * _mtx.mat[9] +  mat[11] * _mtx.mat[10] + mat[15] * _mtx.mat[11];
		ret.mat[12] = mat[0] * _mtx.mat[12] + mat[4] * _mtx.mat[13] + mat[8] *  _mtx.mat[14] + mat[12] * _mtx.mat[15];
		ret.mat[13] = mat[1] * _mtx.mat[12] + mat[5] * _mtx.mat[13] + mat[9] *  _mtx.mat[14] + mat[13] * _mtx.mat[15];
		ret.mat[14] = mat[2] * _mtx.mat[12] + mat[6] * _mtx.mat[13] + mat[10] * _mtx.mat[14] + mat[14] * _mtx.mat[15];
		ret.mat[15] = mat[3] * _mtx.mat[12] + mat[7] * _mtx.mat[13] + mat[11] * _mtx.mat[14] + mat[15] * _mtx.mat[15];

		return ret;
	}

	 __forceinline vector3 operator*(vector3 &_vec)
	{
		vector3 ret;
		ret.x = mat[0] * _vec.x + mat[4] * _vec.y + mat[8] *  _vec.z + mat[12];
		ret.y = mat[1] * _vec.x + mat[5] * _vec.y + mat[9] *  _vec.z + mat[13];
		ret.z = mat[2] * _vec.x + mat[6] * _vec.y + mat[10] * _vec.z + mat[14];
		return ret;
	}

	 __forceinline matrix4 operator*(float _val)
	{
		matrix4 ret;
		for(u32 i=0;i<16;i++)
		{
			ret.mat[i] = mat[i]*_val;
		}
		return ret;
	}

	 __forceinline matrix4& operator*=(matrix4 &m) { return *this = *this * m; }

	 __forceinline void translate(vector3& _vec)
	{
		mat[0] = 1.0; mat[4] = 0.0; mat[8] = 0.0;  mat[12] = _vec.x;
		mat[1] = 0.0; mat[5] = 1.0; mat[9] = 0.0;  mat[13] = _vec.y;
		mat[2] = 0.0; mat[6] = 0.0; mat[10] = 1.0; mat[14] = _vec.z;
		mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
	}

	 __forceinline void rotate_x(float _ang)
	{
		float c = cos(_ang);
		float s = sin(_ang);
		mat[0] = 1.0; mat[4] = 0.0; mat[8] = 0.0;  mat[12] = 0.0;
		mat[1] = 0.0; mat[5] = c;   mat[9] = -s;   mat[13] = 0.0;
		mat[2] = 0.0; mat[6] = s;   mat[10] = c;   mat[14] = 0.0;
		mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
	}

	 __forceinline void rotate_y(float _ang)
	{
		float c = cos(_ang);
		float s = sin(_ang);
		mat[0] = c;   mat[4] = 0.0; mat[8] = s;    mat[12] = 0.0;
		mat[1] = 0.0; mat[5] = 1.0; mat[9] = 0.0;  mat[13] = 0.0;
		mat[2] = -s;  mat[6] = 0.0; mat[10] = c;   mat[14] = 0.0;
		mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
	}

	 __forceinline void rotate_z(float _ang)
	{
		float c = cos(_ang);
		float s = sin(_ang);
		mat[0] = c;   mat[4] = -s;  mat[8] = 0.0;  mat[12] = 0.0;
		mat[1] = s;   mat[5] = c;   mat[9] = 0.0;  mat[13] = 0.0;
		mat[2] = 0.0; mat[6] = 0.0; mat[10] = 1.0; mat[14] = 0.0;
		mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
	}

	 __forceinline void scale(vector3 &_vec) 
	{
		mat[0] = _vec.x; mat[4] = 0.0;	  mat[8] = 0.0;		mat[12] = 0.0;
		mat[1] = 0.0;	 mat[5] = _vec.y; mat[9] = 0.0;		mat[13] = 0.0;
		mat[2] = 0.0;	 mat[6] = 0.0;	  mat[10] = _vec.z; mat[14] = 0.0;
		mat[3] = 0.0;	 mat[7] = 0.0;	  mat[11] = 0.0;	mat[15] = 1.0;
	}

	 __forceinline matrix4 inverse()
	{
		matrix4 ret;
		float idet = 1.0f/determinant();

		ret.mat[0] =   (mat[5] *  mat[10] - mat[9] *  mat[6]) * idet;
		ret.mat[1] =  -(mat[1] *  mat[10] - mat[9] *  mat[2]) * idet;
		ret.mat[2] =   (mat[1] *  mat[6] -  mat[5] *  mat[2]) * idet;
		ret.mat[3] =    0.0;
		ret.mat[4] =  -(mat[4] *  mat[10] - mat[8] *  mat[6]) * idet;
		ret.mat[5] =   (mat[0] *  mat[10] - mat[8] *  mat[2]) * idet;
		ret.mat[6] =  -(mat[0] *  mat[6] -  mat[4] *  mat[2]) * idet;
		ret.mat[7] =    0.0;
		ret.mat[8] =   (mat[4] *  mat[9] -  mat[8] *  mat[5]) * idet;
		ret.mat[9] =  -(mat[0] *  mat[9] -  mat[8] *  mat[1]) * idet;
		ret.mat[10] =  (mat[0] *  mat[5] -  mat[4] *  mat[1]) * idet;
		ret.mat[11] =   0.0;
		ret.mat[12] = -(mat[12] * ret.mat[0] +  mat[13] * ret.mat[4] + mat[14] * ret.mat[8]);
		ret.mat[13] = -(mat[12] * ret.mat[1] +  mat[13] * ret.mat[5] + mat[14] * ret.mat[9]);
		ret.mat[14] = -(mat[12] * ret.mat[2] +  mat[13] * ret.mat[6] + mat[14] * ret.mat[10]);
		ret.mat[15] =   1.0;

		return ret;
	}

	 __forceinline matrix4 transpose() const 
	{
		matrix4 ret;
		ret.mat[0] = mat[0]; ret.mat[4] = mat[1]; ret.mat[8] = mat[2]; ret.mat[12] = mat[3];
		ret.mat[1] = mat[4]; ret.mat[5] = mat[5]; ret.mat[9] = mat[6]; ret.mat[13] = mat[7];
		ret.mat[2] = mat[8]; ret.mat[6] = mat[9]; ret.mat[10] = mat[10]; ret.mat[14] = mat[11];
		ret.mat[3] = mat[12]; ret.mat[7] = mat[13]; ret.mat[11] = mat[14]; ret.mat[15] = mat[15];
		return ret;
	}

	 __forceinline void identity()
	{
		mat[0] = 1.0f; mat[4] = 0.0f; mat[8] = 0.0f;  mat[12] = 0.0f;
		mat[1] = 0.0f; mat[5] = 1.0f; mat[9] = 0.0f;  mat[13] = 0.0f;
		mat[2] = 0.0f; mat[6] = 0.0f; mat[10] = 1.0f; mat[14] = 0.0f;
		mat[3] = 0.0f; mat[7] = 0.0f; mat[11] = 0.0f; mat[15] = 1.0f;
	}

	 __forceinline float determinant()
	{
		float det;

		det =  mat[0] * mat[5] * mat[10];
		det += mat[4] * mat[9] * mat[2];
		det += mat[8] * mat[1] * mat[6];
		det -= mat[8] * mat[5] * mat[2];
		det -= mat[4] * mat[1] * mat[10];
		det -= mat[0] * mat[9] * mat[6];

		return det;
	}

	 __forceinline void perspective(float _fov,float _aspect,float _z_near,float _z_far)
	{
		float y = tan(_fov*pi/360.0f);
		float x = y * _aspect;
		mat[0] = 1.0f/x; mat[4] = 0.0;	  mat[8] = 0.0;									mat[12] = 0.0;
		mat[1] = 0.0;	 mat[5] = 1.0f/y; mat[9] = 0.0;									mat[13] = 0.0;
		mat[2] = 0.0;	 mat[6] = 0.0;	  mat[10] = -(_z_far+_z_near)/(_z_far-_z_near); mat[14]=-(2.0f*_z_far*_z_near)/(_z_far-_z_near);
		mat[3] = 0.0;	 mat[7] = 0.0;	  mat[11] = -1.0;								mat[15] = 0.0;
	}

	 __forceinline void look_at(vector3 &_eye,vector3 &_dir,const vector3 &_up)
	{
		vector3 x,y,z;
		matrix4 m0,m1;
		z = -_dir.normal();
		x = _up.cross(z).normal();
		y = z.cross(x).normal();
		m0.mat[0] = x.x; m0.mat[4] = x.y; m0.mat[8] = x.z; m0.mat[12] = 0.0;
		m0.mat[1] = y.x; m0.mat[5] = y.y; m0.mat[9] = y.z; m0.mat[13] = 0.0;
		m0.mat[2] = z.x; m0.mat[6] = z.y; m0.mat[10] = z.z; m0.mat[14] = 0.0;
		m0.mat[3] = 0.0; m0.mat[7] = 0.0; m0.mat[11] = 0.0; m0.mat[15] = 1.0;
		m1.translate(-_eye);
		*this = m0 * m1;
	}

	 __forceinline float* get_ptr()
	 {
		 return mat;
	 }

	/*operator float*()
	{ 
		return mat; 
	}*/

	//float& operator[](u32 i) 
	//{ 
	//	return mat[i]; 
	//}

	/*operator const float*() const 
	{ 
		return mat; 
	}*/
public:
	union
	{
		float mat[16];
		struct
		{
			float _11, _12, _13, _14;
			float _21, _22, _23, _24;
			float _31, _32, _33, _34;
			float _41, _42, _43, _44;
		};
	};
};

#endif